#include <mc_perception/fusion/sensors_fusion.h>

SensorsFusion::SensorsFusion(int lidar2dNums, int pointcloud2Nums){
    lidar2dNums_ = lidar2dNums;
    pointcloud2Nums_ = pointcloud2Nums;
    int totalNums = lidar2dNums + pointcloud2Nums;
    pointcloudFusionDeque_.resize(totalNums);

    GLOG_INFO << "[SensorsFusion] lidar2dNums: " << lidar2dNums << ", pointcloud2Nums: " << pointcloud2Nums;
}

// 添加雷达数据
void SensorsFusion::addScan2DData(Lidar2DConfig config, Scan2DPtr scan_ptr){
    PointCloudXYZPtr pointcloud_ptr = nullptr;
    if(config.need_undistortion){
        // 获取去除运动畸变后的点云数据
        pointcloud_ptr = lidarUndistor_.getUndistortionLidarData(scan_ptr);
    }
    else{
        // 激光雷达转点云数据
        pointcloud_ptr = msg_convert::convertToPointCloudXYZ(scan_ptr);
    }

    // 转为base_link坐标系
    msg_convert::transformPointXYZ(
        pointcloud_ptr, pointcloud_ptr, config.base_config.transform_matrix
    );
    // 更新数据
    pointcloudFusionDeque_[config.base_config.index] = pointcloud_ptr;
}

// 添加点云数据
void SensorsFusion::addPointCloudData(PointCloudConfig config, PointCloud2Ptr pointcloud2_ptr){
    PointCloudXYZPtr pointcloud_ptr;
    pointcloud_ptr = msg_convert::convertToPointCloudXYZ(pointcloud2_ptr);

    // 更新数据
    int index = lidar2dNums_ + config.base_config.index;
    pointcloudFusionDeque_[index] = pointcloud_ptr;
}

// 添加IMU数据
void SensorsFusion::addIMUData(ImuPtr imu_ptr){
    lidarUndistor_.setImuData(imu_ptr);
}

// 添加里程计数据
void SensorsFusion::addOdomData(OdometryPtr odom_ptr){
    lidarUndistor_.setOdomData(odom_ptr);
}

// 获取融合数据
PointCloud2Ptr SensorsFusion::getFusionData(){
    // 融合数据 时间戳相差超过0.2去除
    PointCloudXYZPtr fusion_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    std::set<std::tuple<float, float, float>> uniquePoints; // 使用集合来存储唯一点, 避免重复点
    
    // 以首个数据时间戳作为基准融合时间戳
    // ros::Time current_time = ros::Time::now();
    bool is_first = false;
    ros::Time current_time;
    for(auto pointcloud_ptr : pointcloudFusionDeque_){
        if(!pointcloud_ptr){
            continue;
        }
        // 时间差大于0.2则跳过
        if(!is_first){
            is_first = true;
            current_time = ros::Time(pointcloud_ptr->header.stamp);
        }
        else{
            if(ros::Time(pointcloud_ptr->header.stamp) - current_time > ros::Duration(0.2)){
                continue;
            }
        }
        // 将当前点云中的所有点添加到融合点云中
        for (const auto &point : pointcloud_ptr->points){
            // 使用 std::tuple 作为唯一标识符
            auto pointTuple = std::make_tuple(point.x, point.y, point.z);
            uniquePoints.insert(pointTuple); // 将唯一点插入集合
        }
    }
    // 将唯一点添加到合并点云中
    for (const auto &pointTuple : uniquePoints){
        pcl::PointXYZ point;
        std::tie(point.x, point.y, point.z) = pointTuple;
        fusion_cloud_ptr->points.push_back(point);
    }
    fusion_cloud_ptr->header.frame_id = "base_link";
    fusion_cloud_ptr->width = fusion_cloud_ptr->points.size();
    fusion_cloud_ptr->height = 1; // 点云为一维

    return msg_convert::convertToPointCloud2(fusion_cloud_ptr);
}